#include "UART.h"

xQueueHandle txq, rxq;


void USART2_IRQHandler(void)
{
	long xHigherPriorityTaskWoken = pdFALSE;
	uint8_t ch;

	if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
	{
		ch = (uint8_t)USART_ReceiveData(USART2);
		xQueueSendToBackFromISR( rxq, &ch, &xHigherPriorityTaskWoken );
	}

	if (USART_GetITStatus(USART2, USART_IT_TXE) != RESET)
		if( xQueueReceiveFromISR( txq, &ch, &xHigherPriorityTaskWoken ) )
			USART_SendData(USART2, ch);
		else
			USART_ITConfig(USART2, USART_IT_TXE, DISABLE);

	portEND_SWITCHING_ISR( xHigherPriorityTaskWoken );
}

uint32_t Usart2GetChar(char *ch)
{
	if(xQueueReceive( rxq, ch, 0 ) == pdPASS)
	{
	  return pdTRUE;
	}
	return pdFALSE;
}

uint32_t Usart2PutChar(char ch)
{
	if( xQueueSend( txq, &ch, 10 ) == pdPASS )
	{
		USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
		return pdTRUE;
	}
	else
	{
		return pdFAIL;
    }
}


static void uart_task(void *params)
{
	  char ch;
	  uint8_t angle = 0;

	  while(1)
	  {
			if(Usart2GetChar(&ch))
			{
				if(ch == 'q')
				{
					angle ++;
					Move(0, 0);
					Move(1, 0);
					Usart2PutChar(ch);
				}
				else if(ch == 'w')
				{
					angle --;
					Move(0, 45);
					Move(1, 45);
					Usart2PutChar(ch);
				}
				else if(ch == 'e')
				{
					angle --;
					Move(0, 90);
					Move(1, 90);
					Usart2PutChar(ch);
				}
				else if(ch == 'r')
				{
					angle --;
					Move(0, 135);
					Move(1, 135);
					Usart2PutChar(ch);
				}
			}
	  }
}

void putstr( char* str)
{
    while (*str != 0)
    	Usart2PutChar(*str++);
}

static void uart_irq_init(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;

	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);

	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
}

static void uart_hw_init(uint32_t baud)
{
	USART_InitTypeDef conf;
	GPIO_InitTypeDef GPIO_InitStruct;

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2|GPIO_Pin_3;
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
	GPIO_Init(GPIOA, &GPIO_InitStruct);

	GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_USART2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_USART2);

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);

	conf.USART_BaudRate = baud;
	conf.USART_WordLength = USART_WordLength_8b;
	conf.USART_StopBits = USART_StopBits_1;
	conf.USART_Parity = USART_Parity_No;
	conf.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	conf.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

	USART_Init(USART2, &conf);

	USART_Cmd(USART2, ENABLE);
}

void uart_task_init(void)
{
	  rxq = xQueueCreate( 128, sizeof( portCHAR ) );
	  txq = xQueueCreate( 128, sizeof( portCHAR ) );

	  xTaskCreate(uart_task, (signed char *)"UART", 100, NULL, 1, NULL );
}

void uart_init(uint32_t baud)
{
	uart_task_init();
	uart_hw_init(baud);
	uart_irq_init();
}
